#include "../include/listener.hpp"


namespace wf 
{
    namespace listener 
    {
        ListenerNode::ListenerNode(const rclcpp::NodeOptions &options) : rclcpp::Node("wf_task1_listener_node", options) 
        {
            std::cout << "starting wf_task1_listener_node..." << std::endl;
            init();
        }

        void ListenerNode::init() 
        {
            // load parameters
            source_frame_ = declare_parameter<std::string>("source_frame", "not declared");
            target_frame_ = declare_parameter<std::string>("target_frame", "not declared");

            /* create subsribers */
            p_tfBuffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
            tf_subsriber_ = std::make_shared<tf2_ros::TransformListener>(*p_tfBuffer_);

            image_subscriber_ = this->create_subscription<Image>("wf_image_raw", 1, [this] (const Image::SharedPtr p_img) {
                ListenerNode::onReceiveImage(p_img);
            });

            pcd2_subscriber_ = this->create_subscription<PointCloud2>("wf_pcd2_raw", 1, [this] (const PointCloud2::SharedPtr p_pcd2) {
                ListenerNode::onReceivePcd2(p_pcd2);
            });

            pose_subscriber_ = this->create_subscription<PoseStamped>("wf_pose_raw", 1, [this] (const PoseStamped::SharedPtr p_pose) {
                ListenerNode::onReceivePose(p_pose);
            });

        }

        void ListenerNode::onReceiveImage(const Image::SharedPtr p_img) 
        {
            std::cout << "receive image, frame: " << ++tot_img_ << ", width: " << p_img->width << ", height: " << p_img->height << std::endl;
        }

        void ListenerNode::onReceivePcd2(const PointCloud2::SharedPtr p_pcd2) 
        {
            std::cout << "receive pcd2, frame: " << ++tot_pcd2_ << ", point num: " << p_pcd2->width << std::endl;
        }

        void ListenerNode::onReceivePose(const PoseStamped::SharedPtr p_pose) {
            std::cout << "receive pose, frame: " << ++tot_pose_ << std::endl;

            source_point_.header.frame_id = source_frame_;
            source_point_.header.stamp = this->now();
            source_point_.point.x = p_pose->pose.position.x;
            source_point_.point.y = p_pose->pose.position.y;
            source_point_.point.z = p_pose->pose.position.z;

            std::cout << "source position: (" << source_point_.point.x << ", " 
                << source_point_.point.y << ", " 
                << source_point_.point.z << ")" << std::endl;

            target_point_ = p_tfBuffer_->transform(source_point_, target_frame_);

            std::cout << "target position: (" << target_point_.point.x << ", " 
                << target_point_.point.y << ", " 
                << target_point_.point.z << ")" << std::endl;
        }
    }
}

// register
RCLCPP_COMPONENTS_REGISTER_NODE(wf::listener::ListenerNode)
